//
// Created by zhuruyi on 2021/3/12.
//

#include "rsm_test.h"
#include "MessageFrame.h"

MessageFrame *test_rsm() {
    RoadsideSafetyMessage roadsideSafetyMessage;

    roadsideSafetyMessage.msgCnt = 10;

    asn_struct_ctx_t ctx;

    OCTET_STRING_t id;
    id._asn_ctx = ctx;
    //construct buff
    id.size = 8;
    //buffer size must be  8
    char rsu_id[] = "aaa-1234";
    id.buf = (uint8_t *) rsu_id;
    roadsideSafetyMessage.id = id;

    //34.744998,113.654938
    Position3D_t pos;
    pos._asn_ctx = ctx;
    //ongitude jingdu
    pos.Long = (Longitude_t) 113.654938;
    pos.lat = (Latitude_t) 34.744998;
    // 海拔 分辨率0.1m   -40
    auto elevation = (Elevation_t) 200.1;
    pos.elevation = &elevation;
    roadsideSafetyMessage.refPos = pos;

    //定义交通参与者的基本安全信息。对应 BSM 消息,该参与者信息由 RSU 探测得到。
    ParticipantData *participantDataArr[1];

    ParticipantData_t participantData;
    //路侧单元检测的交通参与者类型
    participantData.ptcType = ParticipantType_pedestrian;
    participantData.ptcId = 1000;
    //定义路侧交通参与者数据的检测器来源
    participantData.source = SourceType_v2x;
    participantData.id = nullptr;
    participantData.plateNo = nullptr;
    //定义1分钟内的毫秒级时刻。分辨率为1毫秒 范围0-59999， 60000及以上表示未知或无效数值
    participantData.secMark = 2000;

    //定义三维对位置。（相对经纬度和相对高程）
    PositionOffsetLLV_t positionOffsetLlv;
    //PositionOffsetLL_t 经纬度偏差，用来描述一个坐标点的相对位置。提供了7种尺度的描述方式。
    //ll1 对应Position-LL-24B， 24 比特相对经纬度位置,表示当前位置点关于参考位置点的经纬度偏差。由两个 12 比特的经度、纬度偏差值组成
    positionOffsetLlv.offsetLL.present = PositionOffsetLL_PR_position_LL1;
    positionOffsetLlv.offsetLL.choice.position_LL1.lat = (OffsetLL_B12_t) 34.744998;
    positionOffsetLlv.offsetLL.choice.position_LL1.lon = (OffsetLL_B12_t) 113.654938;
    positionOffsetLlv.offsetLL.choice.position_LL1._asn_ctx = ctx;
    positionOffsetLlv.offsetV = nullptr;
    participantData.pos = positionOffsetLlv;

    //PositionConfidenceSet_t
    //定义位置（经纬度和高程）的综合精度
    PositionConfidenceSet_t accuracy;
    //描述了95%置信水平的车辆位置精度
    PositionConfidence_t position_confidence = PositionConfidence_a500m;
    //描述了95%置信水平的车辆度精度
    ElevationConfidence_t elevation_confidence = ElevationConfidence_elev_500_00;
    accuracy.elevation = &elevation_confidence;
    accuracy.pos = position_confidence;
    accuracy._asn_ctx = ctx;
    participantData.accuracy = accuracy;

    participantData.transmission = nullptr;

    //车速大小。分辨率为 0.02 m/s。数值 8191 表示无效数值。
    participantData.speed = 4000;

    //为车辆航向角,即为车头方向与正北方向的顺时针夹角。分辨率为 0.0125° . 0-28800
    participantData.heading = 10000;

    //方向盘转角 向右侧为正 	向左为负  分辨率1.5度 数值127无效
    SteeringWheelAngle_t angle = -5;
    participantData.angle = &angle;

    //描述车辆运行状态的精度 。包括车速精度/ 航向精度和方向盘转角精度
    MotionConfidenceSet_t motion_confidence_set;
    //描述了95%置信水平的速精度
    SpeedConfidence_t speed = SpeedConfidence_prec100ms;
    motion_confidence_set.speedCfd = &speed;
    //描述了95%置信水平的车辆航向精度
    HeadingConfidence_t heading = HeadingConfidence_prec10deg;
    motion_confidence_set.headingCfd = &heading;
    //描述了95%置信水平的方向盘转角精度
    SteeringWheelAngleConfidence_t sw_angle = SteeringWheelAngleConfidence_prec2deg;
    motion_confidence_set.steerCfd = &sw_angle;
    motion_confidence_set._asn_ctx = ctx;
    participantData.motionCfd = &motion_confidence_set;

    //定义车辆四轴加速度。
    // Long 纵向加速度  向前加速为正，反向为负
    // Lat 横向加速度 向右加速为正  反向为负
    // Vert 垂直加速度  沿重力方向向下为正，反向为负
    // Yaw 横摆角速度 顺时针旋转为正，反向为负
    AccelerationSet4Way_t accel_set;
    accel_set._asn_ctx = ctx;
    //Acceleration 定义车辆加速度，分辨率为0.01m/s2 。 数值2001为无效数值
    accel_set.Long = 5;
    accel_set.lat = 3;
    accel_set.vert = 6;
    accel_set.yaw = 10;
    participantData.accelSet = &accel_set;

    //定义车辆尺寸。由车辆长/宽/高度来定义。高度数值为可选项。
    VehicleSize_t size;
    size._asn_ctx = ctx;
    VehicleWidth_t width = 3;
    size.width = width;
    VehicleLength_t length = 5;
    size.length = length;
    VehicleHeight_t height = 3;
    size.height = &height;
    participantData.size = size;

    // 定义车辆的分类，可从各个维度对车辆类型进行定义。目前仅有车辆基本类型一项。
    VehicleClassification_t vehicleClass;
    //基本乘用车类型
    const int BASIC_PASSENGER_VEHICLE_TYPE = 10;
    BasicVehicleClass_t basic_vehicle_class = BASIC_PASSENGER_VEHICLE_TYPE;
    vehicleClass.classification = basic_vehicle_class;
    vehicleClass._asn_ctx = ctx;
    participantData.vehicleClass = &vehicleClass;

    participantData._asn_ctx = ctx;

    participantDataArr[0] = &participantData;
    roadsideSafetyMessage.participants.list.array = participantDataArr;
    roadsideSafetyMessage.participants.list.size = 1;
    roadsideSafetyMessage.participants.list.count = 1;

    //construct MessageFrame
    MessageFrame_t message_frame;
    message_frame.present = MessageFrame_PR_rsmFrame;
    message_frame.choice.rsmFrame = roadsideSafetyMessage;

    xer_fprint(stdout, &asn_DEF_MessageFrame, &message_frame);
    return &message_frame;
}